import matplotlib
import csv
from matplotlib import pyplot as plt
import numpy as np

path = '/home/ubuntu/user_space/KF-pose/readAndroidData/Android1st.csv'
pathCovariance = '/home/ubuntu/user_space/KF-pose/readAndroidData/Covariance.csv'
pathX = '/home/ubuntu/user_space/KF-pose/readAndroidData/Covariance_X.csv'
pathY = '/home/ubuntu/user_space/KF-pose/readAndroidData/Covariance_Y.csv'
pathZ = '/home/ubuntu/user_space/KF-pose/readAndroidData/Covariance_Z.csv'

def main():
    plotAcce()


def plotAcce(Acce, Line):
    fileAndroid = csv.reader(open(path))

    time_raw = Acce[0][1:]
    time_line = Line[0]

    raw_acceX, raw_acceY, raw_acceZ = [], [], []
    next(fileAndroid)
    for row in fileAndroid:
        raw_acceX.append(float(row[7]))
        raw_acceY.append(float(row[8]))
        raw_acceZ.append(float(row[9]))

    fig_acce, axes_acce = plt.subplots(nrows=3)
    axes_acce[0].plot(time_raw, raw_acceX, color='red')
    axes_acce[1].plot(time_raw, raw_acceY, color='red')
    axes_acce[2].plot(time_raw, raw_acceZ, color='red')

    axes_acce[0].plot(time_line, Line[1], color='yellow')
    axes_acce[1].plot(time_line, Line[2], color='yellow')
    axes_acce[2].plot(time_line, Line[3], color='yellow')

    plt.show()

def plotPosition(time_line, Line_Px, Line_Py, Line_Pz, Acce, Gyro, Line):
    fileAndroid = csv.reader(open(path))

    time_posi = Gyro[0][1:]

    Res_Px, Res_Py, Res_Pz = [], [], []
    Raw_Accex, Raw_Accey, Raw_Accez = [], [], []
    Res_Accex, Res_Accey, Res_Accez = [], [], []

    next(fileAndroid)
    for row in fileAndroid:
        Res_Px.append(float(row[13]))
        Res_Py.append(float(row[14]))
        Res_Pz.append(float(row[15]))
        Raw_Accex.append(float(row[10]))
        Raw_Accey.append(float(row[11]))
        Raw_Accez.append(float(row[12]))
        Res_Accex.append(float(row[7]))
        Res_Accey.append(float(row[8]))
        Res_Accez.append(float(row[9]))

    # print(len(Res_Px))
    # print(len(Raw_Accex))

    fig_posi, axes_posi = plt.subplots(nrows=3)
    handle_res, = axes_posi[0].plot(time_posi, Res_Px, color='red')
    axes_posi[1].plot(time_posi, Res_Py, color='red')
    axes_posi[2].plot(time_posi, Res_Pz, color='red')

    handle_line, = axes_posi[0].plot(time_line, Line_Px, color='yellow')
    axes_posi[1].plot(time_line, Line_Py, color='yellow')
    axes_posi[2].plot(time_line, Line_Pz, color='yellow')

    axes_posi[0].legend([handle_res, handle_line], ['res', 'line'], loc='upper right')
    axes_posi[0].set(title='position KF and Linear')

    fig_acce, axes_acce = plt.subplots(nrows=3)
    handle_raw, = axes_acce[0].plot(time_posi, Raw_Accex, color='red')
    axes_acce[1].plot(time_posi, Raw_Accey, color='red')
    axes_acce[2].plot(time_posi, Raw_Accez, color='red')

    handle_aline, = axes_acce[0].plot(time_line, Line[1], color='yellow')
    axes_acce[1].plot(time_line, Line[2], color='yellow')
    axes_acce[2].plot(time_line, Line[3], color='yellow')

    # axes_acce[0].plot(time_posi, Res_Accex, color='blue')
    # axes_acce[1].plot(time_posi, Res_Accey, color='blue')
    # axes_acce[2].plot(time_posi, Res_Accez, color='blue')

    axes_acce[0].legend([handle_raw, handle_aline], ['raw', 'line'], loc='upper right')
    axes_acce[0].set(title='acce KF and Linear')

    plt.show()

def plotCovariance(Acce):
    fileCovariance = csv.reader(open(pathCovariance))

    # time_posi = Acce[0][1:]
    time_posi = np.arange(0, 1745, 1)
    cov_PP, cov_P = [], []

    next(fileCovariance)
    cnt = 1
    for row in fileCovariance:
        if cnt % 3 != 0 and cnt != 1:
            cnt += 1
            continue
        cov_PP.append(float(row[0]))
        cov_P.append(float(row[1]))
        cnt += 1

    fig_cov, axes_cov = plt.subplots(nrows=2)
    axes_cov[0].plot(time_posi, cov_PP, color='blue')
    axes_cov[1].plot(time_posi, cov_P, color='red')

    fileCovX = csv.reader(open(pathX))
    fileCovY = csv.reader(open(pathY))
    fileCovZ = csv.reader(open(pathZ))

    next(fileCovX)
    next(fileCovY)
    next(fileCovZ)
    PP_X, PP_Y, PP_Z, P_X, P_Y, P_Z = [], [], [], [], [], []
    for row in fileCovX:
        print(row)
        PP_X.append(float(row[0]))
        P_X.append(float(row[1]))
    for row in fileCovY:
        PP_Y.append(float(row[0]))
        P_Y.append(float(row[1]))
    for row in fileCovZ:
        PP_Z.append(float(row[0]))
        P_Z.append(float(row[1]))

    time_X = np.arange(0, len(PP_X), 1)
    time_Y = np.arange(0, len(PP_Y), 1)
    time_Z = np.arange(0, len(PP_Z), 1)

    fig_x, axes_x = plt.subplots(nrows=2)
    axes_x[0].plot(time_X, PP_X, color='blue')
    axes_x[1].plot(time_X, P_Y, color='red')
    fig_y, axes_y = plt.subplots(nrows=2)
    axes_y[0].plot(time_Y, PP_Y, color='blue')
    axes_y[1].plot(time_Y, P_Y, color='red')
    fig_z, axes_z = plt.subplots(nrows=2)
    axes_z[0].plot(time_Z, PP_Z, color='blue')
    axes_z[1].plot(time_Z, P_Z, color='red')

    plt.show()


